//TODO:original version had ignore-error feature

#include "sshaperecovery.h"

namespace buola { namespace bwm {

SShapeRecovery::SShapeRecovery(const CMonotonicTime &pt0,const chrono::microseconds &pdt,
                                        double px0,double px1)
{
    t0=pt0;
    dt=pdt;
    x0=px0;
    x1=px1;
    vmax=12*(x1-x0)/to_double_seconds(dt);
}

double SShapeRecovery::GetPos(const CMonotonicTime &pt)
{
    if(pt>t0+dt) return x1;

    double tmt0=to_double_seconds(pt-t0);
    double ldt=to_double_seconds(dt);

    return -vmax*tmt0*tmt0*tmt0/(6*ldt*ldt)+vmax*tmt0*tmt0/(4*ldt)+x0;
}

bool SShapeRecovery::IsFinished(const CMonotonicTime &pt)
{
    return (pt>t0+dt);
}

/*namespace bwm*/ } /*namespace buola*/ }
